#include "AvcUtmPath.h"

void AvcUtmPath::setUtmConversionFactorX(float factor) {
  utmConversionFactorX = factor;
}

void AvcUtmPath::setUtmConversionFactorY(float factor) {
  utmConversionFactorY = factor;
}

void AvcUtmPath::addWaypoint(long lat, long lon) {
  if (numberOfWaypoints < 1 || numberOfWaypoints > MAX_WAYPOINTS - 1) {
    numberOfWaypoints = 0;
    commonLatitude = lat;
    commonLongitude = lon;
  }
  x[numberOfWaypoints] = getX(lon);
  y[numberOfWaypoints] = getY(lat);
  numberOfWaypoints++;
}

void AvcUtmPath::resetWaypoints() {
  numberOfWaypoints = 0;
}

void AvcUtmPath::log(Stream* stream) {
  stream->println("PATH");
  stream->print("  number of waypoints: ");
  stream->println(numberOfWaypoints);
  stream->print("  Common Latitude: ");
  stream->println(commonLatitude);
  stream->print("  Common Longitude: ");
  stream->println(commonLongitude);
  stream->print("  UTM Conversion Factor X: ");
  stream->println(utmConversionFactorX);
  stream->print("  UTM Conversion Factor Y: ");
  stream->println(utmConversionFactorY);
  for (int ii = 0; ii < MAX_WAYPOINTS; ii++) {
    stream->print("Waypoint ");
    stream->print(ii+1);
    stream->println(":");
    stream->print("  x: ");
    stream->println(x[ii]);
    stream->print("  y: ");
    stream->println(y[ii]);
  }
}

void AvcUtmPath::writeToEeprom(int startAddress) {
  byte *path = (byte*) this;
  for (int ii = 0; ii < sizeof(*this); ii++) {
    EEPROM.write(startAddress + ii, path[ii]);
  }
}

